#include <Color.h>
#include <ColorConvert.h>
#include <LinearColorMap.h>
#include <LinearInterpolationMap.h>
#include <ValuePoint.h>
#include <LinearColorMap.h>
#include <Modulation.h>
#include <Average.h>
#include "Tlc5940.h"

#define SONAR_PIN 2
#define SONAR_MIN_DISTANCE 40
#define REDPIN 0
#define GREENPIN 2
#define BLUEPIN 1
#define MAX_COLOR_VALUE 4095
#define MIN_COLOR_VALUE 0
#define DEBUGGING 0

LinearColorMap* colorMap;
Average* valueAverage;
Average* maxAverage;
Modulation* modulation;
int maxDistance;
int maxAverageDistance;
int modulationDistance;


void setup() {
  if (DEBUGGING) {
    Serial.begin(9600);
  }
  pinMode(SONAR_PIN, INPUT);

  initLEDs();
  modulation = newModulation();
  modulation->modulationTime = 3000;
  modulation->minimum = 0.0;
  modulation->maximum = .17;
  maxAverage = newAverageWithSampleSize(5);
  valueAverage = newAverageWithSampleSize(100);
  createLinearColorMap();
  calibrateMaxDistance();
  sendColorToPin(REDPIN, MIN_COLOR_VALUE);
  sendColorToPin(BLUEPIN, 3000);
  Tlc.update();
  delay(1000);
}

void initLEDs() {
  Tlc.init(4095); // remember, it's backwards.
  delay(20);
  sendColorToPin(REDPIN, 3000);
  Tlc.update();
}

int readSonar() {
  return analogRead(0);
  long pulse = pulseIn(SONAR_PIN, HIGH);
  int inches = pulse/147;
  return inches;
}

void calibrateMaxDistance() {
  delay(1000);
  for (int i = 0; i < 5; i++) {
    maxAverage->addValue(readSonar());
    delay(120);
  }
  maxDistance = maxAverage->average;
  modulationDistance = maxDistance * .75;
}

void loop() {
  int distance = readSonar();
  distance = valueAverage->averageWithValue(distance);
  distance = constrain(distance, SONAR_MIN_DISTANCE, maxDistance);
  sendColorForDistance(distance);
  delay(1);
}

void sendColorForDistance(int distance) {
  RGB color;    
  int value = MAX_COLOR_VALUE - map(distance, SONAR_MIN_DISTANCE, maxDistance, MIN_COLOR_VALUE, MAX_COLOR_VALUE);
  color = colorMap->getColorForValue(value);
  // Todo: if the modulated color is different than the distance color, yet we're within the modulation
  // distance, can we start with the modulation color value rather than the colormap value? to avoid jumping
  // too bright?
  if (distance > modulationDistance) {
    color = modulate(color);
  }
  if (DEBUGGING) {
    Serial.print("distance: ");
    Serial.print(distance);
    Serial.print(" maxDistance: ");
    Serial.print(maxDistance);
    Serial.print(" value: ");
    Serial.print(value);
    Serial.println(" ");
  
    delay(100);
  }

  sendColorToPin(REDPIN, color.r);
  sendColorToPin(GREENPIN, color.g);
  sendColorToPin(BLUEPIN, color.b);
  Tlc.update();
}

void sendColorToPin(int pin, int color) {
  int value = constrain(color, MIN_COLOR_VALUE, MAX_COLOR_VALUE);
  // Note: The TLC5940 is backwards: 4095 to 0, dark to bright.
  Tlc.set(pin, MAX_COLOR_VALUE - value);
}

RGB modulate(RGB color) {
  HSV hsvColor = RGBtoHSV(color, MAX_COLOR_VALUE);
  hsvColor.v = modulation->nextModulationValue();
  return HSVtoRGB(hsvColor, MAX_COLOR_VALUE);
}

void createLinearColorMap() {
  colorMap = newLinearColorMap();
  int numberOfPoles = 12;
  int poles[] = { 0, 600, 1040, 1680, 2020, 2580, 3050, 3350, 3500, 3860, 3900, 4000  };
  int red[] = { 100, 800, 4000, 4000, 4000, 0, 0, 0, 0, 4000, 4000, 3000 };
  int green[] = { 0, 0, 0, 0, 4000, 4000, 4000, 0, 0, 0, 0, 3000 };
  int blue[] = { 0, 0, 0, 0, 0, 0, 0, 4000, 4000, 4000, 4000, 3000 };
  for (int i = 0; i < numberOfPoles; i++) {
    colorMap->addRedPoint(poles[i], red[i]);
    colorMap->addGreenPoint(poles[i], green[i]);
    colorMap->addBluePoint(poles[i], blue[i]);
  }
}

